﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using System.Diagnostics;

namespace qtAiGamesLibrary
{
    public class MovingEntity : BaseGameEntity
    {

        #region * STATIC GLOBALS *
        static public readonly double ForwardFacingAngle = 0.95f; // 18 Degrees
        #endregion

        #region * Protected Members *
        protected Vector2 _Velocity;        // the velocity of this object
        protected Vector2 _Heading;         // a normalized vector pointing in the direction the object is heading
        protected Vector2 _Side;            // a fector perpendicular to the heading

        protected double _Mass;             // The weight of this object. it will be in Kg's
        protected double _MaxSpeed;         // The maximum speed that this object can travel!
        protected double _MaxForce;         // The maximum force this object can produce to power itself
        protected double _MaxTurnRate;      // The maximum radians this object can rotate per second (0 - 1)
        #endregion 

        #region * Public Properties *
        public Vector2 Velocity { get { return _Velocity; } set { _Velocity = value; } }
        public Vector2 Heading { get { return _Heading; } set { _Heading = value; } }
        public Vector2 Side { get { return _Side; } set { _Side = value; } }

        public double Mass { get { return _Mass; } set { _Mass = value; } }
        public double MaxSpeed { get { return _MaxSpeed; } set { _MaxSpeed = value; } }
        public double MaxForce { get { return _MaxForce; } set { _MaxForce = value; } }
        public double MaxTurnRate { get { return _MaxTurnRate; } set { _MaxTurnRate = value; } }
        #endregion


        public MovingEntity()
        {
            SetHeading(new Vector2(0.1f, 0.1f));
        }
        /// <summary>
        /// Rotates the entity of a maximum _MaxTurnRate towards the target until
        /// it faces the target. 
        /// Updates both _Heading and _Side
        /// </summary>
        /// <param name="target">The target this is sposed to face</param>
        /// <returns>true, when facing the target!</returns>
        public bool RotateHeadingtoFacePosition(Vector2 target)
        {
            Vector2 toTarget = target - _vPos;
            toTarget.Normalize();

            //first determine the angle between the heading vector and the target
            double angle = Math.Acos(Vector2.Dot(_Heading, toTarget));

            //return true if the player is facing the target
            if (angle < 0.00001) return true;

            //clamp the amount to turn to the max turn rate
            if (angle > _MaxTurnRate) angle = _MaxTurnRate;

            // Create the rotation matrix (we can use Z here cause we are only working in 2d :D
            Matrix RotationMatrix;
            RotationMatrix = Matrix.CreateRotationZ((float)(angle * Utils.Vector2Sign(_Heading, target)));

            // rotate the stuff
            Vector2.Transform(_Heading, RotationMatrix);
            Vector2.Transform(_Velocity, RotationMatrix);

            // recreate the side Vector
            _Side = new Vector2(-_Heading.Y, _Heading.X);

            return false;
        }

        /// <summary>
        /// Set the heading to the vector! Simplesh
        /// </summary>
        /// <param name="New_Heading">Umm, the new heading?</param>
        public void SetHeading(Vector2 New_Heading)
        {
            // Give us a nice error if the length is 0 ;)
            Debug.Assert((New_Heading.LengthSquared() - 1.0f) < 0.00001f, "Heading vector is ZERO!");

            _Heading = New_Heading;

            // recreate the side Vector
            _Side = new Vector2(-_Heading.Y, _Heading.X);
        }

#region * Static Functions *
        /// <summary>
        /// To add more time to the turn around time when pursuing etc. Is not used.
        /// </summary>
        /// <remarks>We need to somehow make that coefficient to be variable depending on the agents turn rate</remarks>
        /// <param name="Agent"></param>
        /// <param name="vTargetPos"></param>
        /// <returns></returns>
        static public double TurnAroundTime(MovingEntity Agent, Vector2 vTargetPos)
        {
            // Normalised vector to target pos
            Vector2 toTarget = Vector2.Normalize((vTargetPos - Agent.vPos));

            double dotProduct = Vector2.Dot(Agent.Heading, toTarget);

            double coefficient = 0.5f;

            return (dotProduct - 1.0d) * (-coefficient);
        }
#endregion
    }
}
